﻿using System;
using System.IO.Ports;
using System.Text;
using System.Threading;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.Graphics.Simple128x64;

namespace Rosvemv2
{
    public class Program
    {
        //Serial connection between robot and control centre
        private static SerialPort _controlCentre;

        //Display graphics painter
        private static GPainter _display;

        //There to ensure that the full booting messages are displayed while booting
        private static bool _doneBooting;

        //Send and recieve bytes for the serial data
        private static byte[] _rxData;
        private static byte[] _txData;

        //Motor 1 variables
        private static PWM _pwm1;
        private static string _dirM1;
        private static OutputPort _dir1;

        //Motor 2 variables
        private static PWM _pwm2;
        private static string _dirM2;
        private static OutputPort _dir2;

        //The speed of the motors
        private static sbyte _motorSpeed;

        //Data received from the serial connection
        private static string _dataReceived;

        //Split version of the dataReveived
        private static string[] _dataLines;

        //Battery voltage pin (an0)
        private static AnalogIn _batteryVoltage;

        /// <summary>
        /// Main method. Initialises everything
        /// </summary>
        private static void Main()
        {
            //UART Display
            FEZ_Extensions.Graphical128x64Display.Initialize(FEZ_Pin.Digital.UEXT3, FEZ_Pin.Digital.UEXT4, FEZ_Pin.Digital.UEXT10, SPI.SPI_module.SPI2);
            _display = new GPainter();

            PrintToDisplay(0, 0, "Rosvem v1.1");
            PrintToDisplay(0, 10, "Robot initializing");

            //Add event for when receiving data and open com port
            _controlCentre = new SerialPort("COM1", 115200);
            _controlCentre.DataReceived += ControlCentreDataReceived;
            _controlCentre.Open();

            PrintToDisplay(0, 20, "Control port opened");

            //Motor 1 pins
            _pwm1 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di8);
            _dir1 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di9, false);

            //Motor 2 pins
            _pwm2 = new PWM((PWM.Pin)FEZ_Pin.PWM.Di6);
            _dir2 = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.Di7, false);

            PrintToDisplay(0, 30, "Motor pins enabled");

            //Reading battery voltage
            _batteryVoltage = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An0);
            _batteryVoltage.SetLinearScale(0, 3300);

            PrintToDisplay(0, 40, "Battery pin enabled");

            //Sending data to control centre
            Thread sendValuesToCommandCenter = new Thread(SendData);
            sendValuesToCommandCenter.Start();

            PrintToDisplay(0, 50, "Ready to send data");

            Thread.Sleep(1000);

            //No garbage collector messages in debug window
            Debug.EnableGCMessages(false); 

            //Clear display and make doneBooting true. Now we can add text
            _display.Clear();
            _doneBooting = true;

            //Take a nap
            Thread.Sleep(Timeout.Infinite);
        }

        /// <summary>
        /// Gets triggered when we receive data over the serial connection
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private static void ControlCentreDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            _rxData = new byte[_controlCentre.BytesToRead];
            _controlCentre.Read(_rxData, 0, _rxData.Length);
            _dataReceived = new String(Encoding.UTF8.GetChars(_rxData));

            //Every piece of data is seperated by ; . So let's split it back to loose data
            _dataLines = _dataReceived.Split(';');

            //If we are receiving the correct length of data
            if (_dataLines.Length == 4)
            {
                _dirM1 = _dataLines[0];
                _dirM2 = _dataLines[2];

                //Which direction for motor 1?
                switch (_dirM1)
                {
                    case "L":
                        _dir1.Write(true);
                        break;
                    case "R":
                        _dir1.Write(false);
                        break;
                }

                //Which direction for motor 2?
                switch (_dirM2)
                {
                    case "L":
                        _dir2.Write(true);
                        break;
                    case "R":
                        _dir2.Write(false);
                        break;
                }

                _motorSpeed = Convert.ToSByte(_dataLines[1]);

                //Motor speed is max 99. To ensure that we have a full stop (100), this if statement is needed
                if (_motorSpeed > 95 && _motorSpeed < 100)
                {
                    _motorSpeed = 100;
                }

                //Set the pwm value to the pwm pins
                _pwm1.Set(1000, (byte)(100 - System.Math.Abs(_motorSpeed)));
                _pwm2.Set(1000, (byte)(100 - System.Math.Abs(_motorSpeed)));
            }

            //Everything null
            _rxData = null;
            _dataReceived = null;
            _dataLines = null;
        }

        /// <summary>
        /// Send data over serial to the control centre
        /// </summary>
        private static void SendData()
        {
            Random random = new Random();
            while (true)
            {
                //Reading battery voltage from an0
                double voltage = ReadBatteryVoltage();

                //Data to send
                string sendData = "V+: " + voltage + "V;Amp1: " + random.Next(20) + "A;Amp2: " + random.Next(20) + "A; Temp: " + random.Next(40) + "C";

                //Add data to byte array and send it over serial
                _txData = new byte[sendData.Length];
                _txData = Encoding.UTF8.GetBytes(sendData);
                _controlCentre.Write(_txData, 0, _txData.Length);

                //Write the voltage to the display
                StatusDisplay(voltage, 0);
                Thread.Sleep(100);
            }
        }

        /// <summary>
        /// reads the battery voltage
        /// </summary>
        /// <returns>Returns voltage in double</returns>
        private static double ReadBatteryVoltage()
        {
            double analogVoltage = _batteryVoltage.Read();
            //Returns the read voltage * the factor
            //Factor = battery voltage / read voltage by fez board
            return ((analogVoltage / 1000) * 7.6600985221674876847290640394089);
        }

        /// <summary>
        /// Prints text to the gpainter to show on the display
        /// </summary>
        /// <param name="x">x coordinate</param>
        /// <param name="y">y coordinate</param>
        /// <param name="text">the text</param>
        private static void PrintToDisplay(int x, int y, string text)
        {
            //Print to corresponding x and y coordinates
            _display.Print(x, y, text);

            //Flush display so that we see the text
            FEZ_Extensions.Graphical128x64Display.Flush(_display.vram);
        }

        /// <summary>
        /// Writes robot data to display
        /// </summary>
        /// <param name="voltage">Voltage</param>
        /// <param name="temp">Temperature</param>
        private static void StatusDisplay(double voltage, double temp)
        {
            //If the booting is done we can start writing
            if (_doneBooting)
            {
                //Clear display graphics
                _display.Clear();

                //Print data
                PrintToDisplay(0, 0, "Rosvem v1.1");
                PrintToDisplay(0, 10, "_____________________");
                PrintToDisplay(0, 20, "Voltage: " + voltage.ToString());
                PrintToDisplay(0, 30, "Temperature: " + temp);
            }
        }

    }
}
